/*
 * SpaceSearch.cpp
 *
 *  Created on: Dec 9, 2009
 *      Author: svilen
 */

#include "SpaceSearch.h"
#include <ctime>
#include <vector>
#include <boost/random.hpp>

using namespace boost;
using namespace std;

SpaceSearch::SpaceSearch(vision_client *_client) {
	client = _client;
	iteration = 0;
}

SpaceSearch::~SpaceSearch() {

}

double SpaceSearch::Fitness(State* state)
{
	return client->projectionFitness(state->alpha, state->beta, state->dist, state->diam);
}

double SpaceSearch::RandomDouble(double min, double max)
{
	//Add noise - for now uniform "white" noise
	boost::mt19937 randgen(static_cast<unsigned int>(std::time(0)));
	boost::uniform_real<double> noise_dist(min, max);
	variate_generator<mt19937, uniform_real<double> > nD(randgen, noise_dist);
	return nD();
}
//------------------------------------------------------------------------------
bool StateEq(State state1, State state2)
{
	return (state1.alpha == state2.alpha && state1.beta == state2.beta
			&& state1.diam == state2.diam && state1.dist == state2.dist);
}


//-------------------------------------------------------------------------------
GradientSearch::GradientSearch(vision_client* _client) : SpaceSearch(_client)
{

	neighbour_states = (State*) malloc(9*sizeof(State));
}

GradientSearch::~GradientSearch()
{
	free(neighbour_states);
	//for(it = neighbours->begin(); it != neighbours->end(); it++)
	//	free(*it);
}

void GradientSearch::Init()
{
	curr_state.diam = BALL_DIAMETER;
	/*curr_state.alpha = RandomDouble(-M_PI/2, M_PI/2);
	curr_state.beta = RandomDouble(-M_PI/2, M_PI/2);
	curr_state.dist = RandomDouble(0, MAX_DISTANCE);*/

	curr_state.alpha = 0;
	curr_state.beta = 0;
	curr_state.dist = 383;
}

void GradientSearch::GenerateSuccessors()
{
	neighbours.clear();

	int cnt=0;
	for(int i=-1; i<2; i++)
		for(int j=-1; j<2; j++)
			for(int k=-1; k<2; k++)
			{
				neighbour_states[cnt].alpha = curr_state.alpha + k * ANGLE_RESOLUTION;
				neighbour_states[cnt].beta = curr_state.beta + j * ANGLE_RESOLUTION;
				neighbour_states[cnt].dist = curr_state.dist + i * DISTANCE_RESOLUTION;
				neighbour_states[cnt].diam = curr_state.diam;
				cnt++;

				neighbours.push_back(&neighbour_states[cnt]);
			}
}

void GradientSearch::DoSearch(float &alpha, float &beta, float &dist, float &diam)
{
	double max_fitness = -1;
	State max_state;
	double curr_fitness;
	bool restart = true;

	vector<struct State*>::iterator it;

	while(true)
	{
		iteration++;
		if(iteration % 1000 == 0)
			cout << "Iteration: " << iteration << endl;

		if(restart)
			Init();

		GenerateSuccessors();

		for(it = neighbours.begin(); it != neighbours.end();it++)
		{
			curr_fitness = Fitness(*it);
			if(curr_fitness > max_fitness)
			{
				max_fitness = curr_fitness;
				max_state = **(it);
			}
		}

		//Check if max_fitness better than fitness of current state
		if(!StateEq(max_state,curr_state))
		{
			curr_state = max_state;
			restart = false;
			continue;
		}
		//Means maximum is at current state
		else
		{
			//That's how we define a global maximum
			if(max_fitness >= good_threshold)
			{
				alpha = curr_state.alpha;
				beta = curr_state.beta;
				dist = curr_state.dist;
				diam = curr_state.diam;
				return;
			}
			//Local maximum - restart
			else
			{
				restart = true;
				continue;
			}
		}
	}

}
